/**
 * @file      ICM20948.c
 * @brief     ICM-20948 9轴运动跟踪设备驱动程序实现
 * @version   1.0
 * @date      2025-07-15
 */

#include "ICM20948.h"
#include "../../CLOCK/clock.h"
#include <string.h>
#include <stdio.h>
#include "../../software/UART_XDS_PC/UART_XDS_PC.h"

#define ICM_DEBUG_PRINT(...) UART_XDS_PC_DefaultPrintf(__VA_ARGS__)

/*************************************************************************************************/
/*                                         私有定义                                              */
/*************************************************************************************************/
#define I2C_TIMEOUT 1000 // I2C超时计数

/*************************************************************************************************/
/*                                         私有变量                                              */
/*************************************************************************************************/
static float g_accel_res;
static float g_gyro_res;
static float g_mag_res;

static volatile bool g_data_ready = false;
static uint8_t g_current_bank = 0xFF; // 初始设为无效bank，强制第一次切换

/*************************************************************************************************/
/*                                       私有函数声明                                            */
/*************************************************************************************************/
int8_t ICM20948_WriteReg(uint8_t reg, uint8_t value);
int8_t ICM20948_ReadReg(uint8_t reg, uint8_t* value);
int8_t ICM20948_ReadMultipleRegs(uint8_t reg, uint8_t* buffer, uint8_t length);
int8_t ICM20948_SwitchBank(uint8_t bank);

static int8_t ICM20948_Mag_WriteReg(uint8_t reg, uint8_t value);
static int8_t ICM20948_Mag_ReadMultipleRegs(uint8_t reg, uint8_t* buffer, uint8_t length);

/*************************************************************************************************/
/*                                       I2C基础函数实现                                         */
/*************************************************************************************************/

/**
 * @brief 切换ICM20948的用户Bank
 * @param bank 要切换的Bank (USER_BANK_0, USER_BANK_1, USER_BANK_2, USER_BANK_3)
 * @return 0: 成功, -1: 失败
 */
int8_t ICM20948_SwitchBank(uint8_t bank) {
    if (g_current_bank == bank) {
        return 0; // 已经在目标bank，无需切换
    }
    if (ICM20948_WriteReg(REG_BANK_SEL, bank) != 0) {
        return -1;
    }
    g_current_bank = bank;
    return 0;
}

/**
 * @brief 写单个寄存器
 */
int8_t ICM20948_WriteReg(uint8_t reg, uint8_t value) {
    uint8_t txData[2] = {reg, value};
    DL_I2C_startControllerTransfer(I2C_ICM20943_INST, ICM20948_I2C_ADDR, DL_I2C_CONTROLLER_DIRECTION_TX, 2);
    DL_I2C_fillControllerTXFIFO(I2C_ICM20943_INST, txData, 2);
    
    uint32_t timeout = I2C_TIMEOUT;
    while(DL_I2C_getControllerStatus(I2C_ICM20943_INST) & DL_I2C_CONTROLLER_STATUS_BUSY_BUS) {
        if (--timeout == 0) return -1;
    }
    return 0;
}

/**
 * @brief 读单个寄存器
 */
int8_t ICM20948_ReadReg(uint8_t reg, uint8_t* value) {
    DL_I2C_startControllerTransfer(I2C_ICM20943_INST, ICM20948_I2C_ADDR, DL_I2C_CONTROLLER_DIRECTION_TX, 1);
    DL_I2C_fillControllerTXFIFO(I2C_ICM20943_INST, &reg, 1);

    uint32_t timeout = I2C_TIMEOUT;
    while(DL_I2C_getControllerStatus(I2C_ICM20943_INST) & DL_I2C_CONTROLLER_STATUS_BUSY_BUS) {
        if (--timeout == 0) return -1;
    }

    DL_I2C_startControllerTransfer(I2C_ICM20943_INST, ICM20948_I2C_ADDR, DL_I2C_CONTROLLER_DIRECTION_RX, 1);

    timeout = I2C_TIMEOUT;
    while(DL_I2C_isControllerRXFIFOEmpty(I2C_ICM20943_INST)) {
         if (--timeout == 0) return -1;
    }
    *value = DL_I2C_receiveControllerData(I2C_ICM20943_INST);
    
    timeout = I2C_TIMEOUT;
    while(DL_I2C_getControllerStatus(I2C_ICM20943_INST) & DL_I2C_CONTROLLER_STATUS_BUSY_BUS) {
        if (--timeout == 0) return -1;
    }
    return 0;
}

/**
 * @brief 读多个寄存器
 */
int8_t ICM20948_ReadMultipleRegs(uint8_t reg, uint8_t* buffer, uint8_t length) {
    DL_I2C_startControllerTransfer(I2C_ICM20943_INST, ICM20948_I2C_ADDR, DL_I2C_CONTROLLER_DIRECTION_TX, 1);
    DL_I2C_fillControllerTXFIFO(I2C_ICM20943_INST, &reg, 1);

    uint32_t timeout = I2C_TIMEOUT;
    while(DL_I2C_getControllerStatus(I2C_ICM20943_INST) & DL_I2C_CONTROLLER_STATUS_BUSY_BUS) {
        if (--timeout == 0) return -1;
    }

    DL_I2C_startControllerTransfer(I2C_ICM20943_INST, ICM20948_I2C_ADDR, DL_I2C_CONTROLLER_DIRECTION_RX, length);

    for (uint8_t i = 0; i < length; i++) {
        timeout = I2C_TIMEOUT;
        while(DL_I2C_isControllerRXFIFOEmpty(I2C_ICM20943_INST)) {
             if (--timeout == 0) return -1;
        }
        buffer[i] = DL_I2C_receiveControllerData(I2C_ICM20943_INST);
    }

    timeout = I2C_TIMEOUT;
    while(DL_I2C_getControllerStatus(I2C_ICM20943_INST) & DL_I2C_CONTROLLER_STATUS_BUSY_BUS) {
        if (--timeout == 0) return -1;
    }
    return 0;
}


/*************************************************************************************************/
/*                                       公共函数实现                                            */
/*************************************************************************************************/

/**
 * @brief 初始化ICM20948传感器
 * @return 0: 成功, -1: 失败
 */
int8_t ICM20948_Init(void) {
    ICM_DEBUG_PRINT("[ICM] Starting initialization...\r\n");
    // 1. 检查设备ID
    if (ICM20948_CheckID() != 0) {
        ICM_DEBUG_PRINT("[ICM] Check ID failed!\r\n");
        return -1;
    }
    ICM_DEBUG_PRINT("[ICM] Check ID OK.\r\n");

    // 2. 软复位传感器
    ICM20948_Reset();
    mspm0_delay_ms(50); // 等待复位完成
    ICM_DEBUG_PRINT("[ICM] Sensor reset.\r\n");

    // 3. 从休眠模式唤醒
    ICM20948_WakeUp();
    mspm0_delay_ms(50);
    ICM_DEBUG_PRINT("[ICM] Sensor wakeup.\r\n");

    // 4. 设置时钟源为自动选择
    ICM20948_SwitchBank(USER_BANK_0);
    ICM20948_WriteReg(PWR_MGMT_1, PWR_MGMT_1_CLK_BEST_AVAIL);
    mspm0_delay_ms(10);
    ICM_DEBUG_PRINT("[ICM] Clock source set.\r\n");

    // 使能加速度计和陀螺仪
    ICM20948_WriteReg(PWR_MGMT_2, 0x00);
    mspm0_delay_ms(10);
    ICM_DEBUG_PRINT("[ICM] Accel & Gyro enabled.\r\n");

    // 显式配置和使能数据就绪中断
    // INT_PIN_CFG: 中断锁存, 任何读操作清除中断
    ICM20948_WriteReg(INT_PIN_CFG, 0x30); 
    // INT_ENABLE_1: 使能原始数据就绪中断
    ICM20948_WriteReg(INT_ENABLE_1, 0x01);
    ICM_DEBUG_PRINT("[ICM] DRDY interrupt enabled.\r\n");

    // 5. 设置满量程范围
    ICM20948_SetAccelFSR(ACCEL_FS_4G);  // 示例：设置为±4g
    ICM_DEBUG_PRINT("[ICM] Accel FSR set.\r\n");
    ICM20948_SetGyroFSR(GYRO_FS_2000DPS); // 示例：设置为±2000dps
    ICM_DEBUG_PRINT("[ICM] Gyro FSR set.\r\n");

    // 6. 设置采样率
    ICM20948_SetAccelSampleRateDiv(10); // 示例：~100Hz
    ICM_DEBUG_PRINT("[ICM] Accel sample rate set.\r\n");
    ICM20948_SetGyroSampleRateDiv(10);  // 示例：~100Hz
    ICM_DEBUG_PRINT("[ICM] Gyro sample rate set.\r\n");

    // 7. 初始化磁力计
    if (ICM20948_MagInit(MAG_MODE_CONT_100HZ) != 0) {
        ICM_DEBUG_PRINT("[ICM] Mag init failed!\r\n");
        return -1; // 磁力计初始化失败
    }
    ICM_DEBUG_PRINT("[ICM] Mag init OK.\r\n");
    
    ICM_DEBUG_PRINT("[ICM] Initialization complete.\r\n");
    return 0;
}

/**
 * @brief 检查设备ID
 */
int8_t ICM20948_CheckID(void) {
    uint8_t who_am_i = 0;
    ICM20948_SwitchBank(USER_BANK_0);
    if (ICM20948_ReadReg(WHO_AM_I_ICM20948, &who_am_i) != 0) {
        ICM_DEBUG_PRINT("[ICM] Read WHO_AM_I failed.\r\n");
        return -1;
    }
    ICM_DEBUG_PRINT("[ICM] WHO_AM_I = 0x%02X\r\n", who_am_i);
    if (who_am_i != ICM20948_DEVICE_ID) {
        return -1;
    }
    return 0;
}

/**
 * @brief 软复位ICM20948
 */
void ICM20948_Reset(void) {
    ICM20948_SwitchBank(USER_BANK_0);
    uint8_t val;
    ICM20948_ReadReg(PWR_MGMT_1, &val);
    ICM20948_WriteReg(PWR_MGMT_1, val | (1 << PWR_MGMT_1_DEVICE_RESET_BIT));
}

/**
 * @brief 唤醒ICM20948
 */
void ICM20948_WakeUp(void) {
    ICM20948_SwitchBank(USER_BANK_0);
    uint8_t val;
    ICM20948_ReadReg(PWR_MGMT_1, &val);
    ICM20948_WriteReg(PWR_MGMT_1, val & ~(1 << PWR_MGMT_1_SLEEP_BIT));
}

/**
 * @brief 设置加速度计满量程范围
 */
void ICM20948_SetAccelFSR(ICM20948_AccelFSR_t fsr) {
    ICM20948_SwitchBank(USER_BANK_2);
    uint8_t val;
    ICM20948_ReadReg(ACCEL_CONFIG, &val);
    val &= ~0x06; // 清除FSR位
    
    switch (fsr) {
        case ACCEL_FS_2G:
            g_accel_res = 2.0f / 32768.0f;
            val |= ACCEL_FS_SEL_2G;
            break;
        case ACCEL_FS_4G:
            g_accel_res = 4.0f / 32768.0f;
            val |= ACCEL_FS_SEL_4G;
            break;
        case ACCEL_FS_8G:
            g_accel_res = 8.0f / 32768.0f;
            val |= ACCEL_FS_SEL_8G;
            break;
        case ACCEL_FS_16G:
            g_accel_res = 16.0f / 32768.0f;
            val |= ACCEL_FS_SEL_16G;
            break;
    }
    ICM20948_WriteReg(ACCEL_CONFIG, val);
}

/**
 * @brief 设置陀螺仪满量程范围
 */
void ICM20948_SetGyroFSR(ICM20948_GyroFSR_t fsr) {
    ICM20948_SwitchBank(USER_BANK_2);
    uint8_t val;
    ICM20948_ReadReg(GYRO_CONFIG_1, &val);
    val &= ~0x06; // 清除FSR位

    switch (fsr) {
        case GYRO_FS_250DPS:
            g_gyro_res = 250.0f / 32768.0f;
            val |= GYRO_FS_SEL_250DPS;
            break;
        case GYRO_FS_500DPS:
            g_gyro_res = 500.0f / 32768.0f;
            val |= GYRO_FS_SEL_500DPS;
            break;
        case GYRO_FS_1000DPS:
            g_gyro_res = 1000.0f / 32768.0f;
            val |= GYRO_FS_SEL_1000DPS;
            break;
        case GYRO_FS_2000DPS:
            g_gyro_res = 2000.0f / 32768.0f;
            val |= GYRO_FS_SEL_2000DPS;
            break;
    }
    ICM20948_WriteReg(GYRO_CONFIG_1, val);
}

/**
 * @brief 设置加速度计采样率分频
 */
void ICM20948_SetAccelSampleRateDiv(uint16_t div) {
    ICM20948_SwitchBank(USER_BANK_2);
    uint8_t div_h = (uint8_t)(div >> 8);
    uint8_t div_l = (uint8_t)(div & 0xFF);
    ICM20948_WriteReg(ACCEL_SMPLRT_DIV_1, div_h);
    ICM20948_WriteReg(ACCEL_SMPLRT_DIV_2, div_l);
}

/**
 * @brief 设置陀螺仪采样率分频
 */
void ICM20948_SetGyroSampleRateDiv(uint8_t div) {
    ICM20948_SwitchBank(USER_BANK_2);
    ICM20948_WriteReg(GYRO_SMPLRT_DIV, div);
}

/**
 * @brief 初始化磁力计 (AK09916)
 */
int8_t ICM20948_MagInit(ICM20948_MagMode_t mode) {
    // 1. 使能I2C主机模式
    ICM20948_SwitchBank(USER_BANK_0);
    uint8_t user_ctrl_val;
    ICM20948_ReadReg(USER_CTRL, &user_ctrl_val);
    ICM20948_WriteReg(USER_CTRL, user_ctrl_val | (1 << USER_CTRL_I2C_MST_EN_BIT));

    // 2. 设置I2C主机时钟频率 (400kHz)
    ICM20948_SwitchBank(USER_BANK_3);
    ICM20948_WriteReg(I2C_MST_CTRL, 0x07);

    // 3. 检查磁力计ID
    if (ICM20948_CheckMagID() != 0) {
        return -1;
    }

    // 4. 复位磁力计
    ICM20948_Mag_WriteReg(AK09916_CNTL3, 0x01);
    mspm0_delay_ms(100);

    // 5. 设置磁力计工作模式
    ICM20948_Mag_WriteReg(AK09916_CNTL2, mode);
    
    // 6. 配置I2C SLV0以连续读取磁力计数据
    ICM20948_SwitchBank(USER_BANK_3);
    ICM20948_WriteReg(I2C_SLV0_ADDR, AK09916_I2C_ADDR | 0x80); // 读地址
    ICM20948_WriteReg(I2C_SLV0_REG, AK09916_HXL); // 从HXL寄存器开始读
    ICM20948_WriteReg(I2C_SLV0_CTRL, 0x80 | 8); // 使能，读取8个字节 (HXL to ST2)
    
    g_mag_res = 0.15f; // AK09916的灵敏度是 0.15uT/LSB
    
    return 0;
}

/**
 * @brief 检查磁力计ID
 */
int8_t ICM20948_CheckMagID(void) {
    uint8_t mag_id[2];
    if (ICM20948_Mag_ReadMultipleRegs(AK09916_WIA1, mag_id, 2) != 0) {
        ICM_DEBUG_PRINT("[ICM] Mag Read ID failed.\r\n");
        return -1;
    }
    ICM_DEBUG_PRINT("[ICM] Mag WIA1=0x%02X, WIA2=0x%02X\r\n", mag_id[0], mag_id[1]);
    if (mag_id[1] != AK09916_DEVICE_ID) {
        return -1;
    }
    return 0;
}

/**
 * @brief 磁力计写单个寄存器
 */
static int8_t ICM20948_Mag_WriteReg(uint8_t reg, uint8_t value) {
    ICM20948_SwitchBank(USER_BANK_3);
    ICM20948_WriteReg(I2C_SLV0_ADDR, AK09916_I2C_ADDR); // 写地址
    ICM20948_WriteReg(I2C_SLV0_REG, reg);
    ICM20948_WriteReg(I2C_SLV0_DO, value);
    ICM20948_WriteReg(I2C_SLV0_CTRL, 0x81); // 使能，写1个字节

    mspm0_delay_ms(10); // 等待写入完成
    return 0;
}

/**
 * @brief 磁力计读多个寄存器
 */
static int8_t ICM20948_Mag_ReadMultipleRegs(uint8_t reg, uint8_t* buffer, uint8_t length) {
    ICM20948_SwitchBank(USER_BANK_3);
    ICM20948_WriteReg(I2C_SLV0_ADDR, AK09916_I2C_ADDR | 0x80); // 读地址
    ICM20948_WriteReg(I2C_SLV0_REG, reg);
    ICM20948_WriteReg(I2C_SLV0_CTRL, 0x80 | length); // 使能，读length个字节

    mspm0_delay_ms(length * 2); // 等待足够的时间来完成读取

    ICM20948_SwitchBank(USER_BANK_0);
    return ICM20948_ReadMultipleRegs(EXT_SLV_SENS_DATA_00, buffer, length);
}

/**
 * @brief 读取所有传感器原始数据
 */
void ICM20948_ReadRawData(ICM20948_RawData_t *data) {
    uint8_t buffer[22];
    ICM20948_SwitchBank(USER_BANK_0);
    ICM20948_ReadMultipleRegs(ACCEL_XOUT_H, buffer, 22);

    char debug_buf[128];
    snprintf(debug_buf, sizeof(debug_buf), 
        "[ICM RAW] AX:%02X%02X AY:%02X%02X AZ:%02X%02X GX:%02X%02X GY:%02X%02X GZ:%02X%02X",
        buffer[0], buffer[1], buffer[2], buffer[3], buffer[4], buffer[5],
        buffer[6], buffer[7], buffer[8], buffer[9], buffer[10], buffer[11]);
    ICM_DEBUG_PRINT("%s\r\n", debug_buf);
    snprintf(debug_buf, sizeof(debug_buf), 
        "[ICM RAW] MX:%02X%02X MY:%02X%02X MZ:%02X%02X ST1:%02X ST2:%02X",
        buffer[16], buffer[15], buffer[18], buffer[17], buffer[20], buffer[19],
        buffer[14], buffer[21]);
    ICM_DEBUG_PRINT("%s\r\n", debug_buf);

    data->accel_x = ((int16_t)buffer[0] << 8) | buffer[1];
    data->accel_y = ((int16_t)buffer[2] << 8) | buffer[3];
    data->accel_z = ((int16_t)buffer[4] << 8) | buffer[5];
    data->gyro_x = ((int16_t)buffer[6] << 8) | buffer[7];
    data->gyro_y = ((int16_t)buffer[8] << 8) | buffer[9];
    data->gyro_z = ((int16_t)buffer[10] << 8) | buffer[11];
    data->temp = ((int16_t)buffer[12] << 8) | buffer[13];
    
    // 磁力计数据存储在EXT_SLV_SENS_DATA中
    // buffer[14] is ST1
    data->mag_x = ((int16_t)buffer[16] << 8) | buffer[15]; // LSB first
    data->mag_y = ((int16_t)buffer[18] << 8) | buffer[17];
    data->mag_z = ((int16_t)buffer[20] << 8) | buffer[19];
    // buffer[21] is ST2,需要检查数据是否有效
}

/**
 * @brief 读取所有传感器数据并进行单位转换
 */
void ICM20948_ReadData(ICM20948_Data_t *data) {
    ICM20948_RawData_t raw;
    ICM20948_ReadRawData(&raw);

    data->accel_x_g = (float)raw.accel_x * g_accel_res;
    data->accel_y_g = (float)raw.accel_y * g_accel_res;
    data->accel_z_g = (float)raw.accel_z * g_accel_res;

    data->gyro_x_dps = (float)raw.gyro_x * g_gyro_res;
    data->gyro_y_dps = (float)raw.gyro_y * g_gyro_res;
    data->gyro_z_dps = (float)raw.gyro_z * g_gyro_res;

    data->mag_x_ut = (float)raw.mag_x * g_mag_res;
    data->mag_y_ut = (float)raw.mag_y * g_mag_res;
    data->mag_z_ut = (float)raw.mag_z * g_mag_res;

    // 温度转换公式: Temp_C = (TEMP_OUT / 333.87) + 21
    data->temp_c = ((float)raw.temp / 333.87f) + 21.0f;
    data->timestamp = tick_ms;
}

/**
 * @brief 数据就绪(DRDY)中断处理函数
 */
void ICM20948_DRDY_Handler(void) {
    g_data_ready = true;
}

/**
 * @brief 判断ICM20948是否数据就绪
 */
bool ICM20948_IsDataReady(void) {
    if (g_data_ready) {
        g_data_ready = false;
        return true;
    }
    return false;
} 

/**
 * @brief 向DMP程序内存写入数据
 * @param memory_addr 内存地址
 * @param data 数据缓冲区
 * @param size 数据大小(字节)
 * @return 0: 成功, 非0: 错误代码
 * @note 此函数用于DMP固件加载
 */
int8_t ICM20948_WriteMemory(uint16_t memory_addr, const uint8_t *data, uint16_t size) {
    int8_t result;
    uint8_t bank;
    uint8_t mem_addr_reg;
    uint8_t mem_data_reg;
    uint8_t tmp[2];
    
    // 基于内存地址，选择正确的寄存器
    if (memory_addr < 0x2000) {
        // DMP程序内存
        bank = USER_BANK_0;
        mem_addr_reg = 0x7C; // MEM_START_ADDR
        mem_data_reg = 0x7D; // MEM_R_W
    } else {
        // 地址超出范围
        return -1;
    }
    
    // 设置内存银行
    ICM20948_SwitchBank(bank);
    
    // 分块写入
    uint16_t bytes_written = 0;
    while (bytes_written < size) {
        // 设置内存地址
        tmp[0] = (uint8_t)((memory_addr + bytes_written) >> 8);
        tmp[1] = (uint8_t)((memory_addr + bytes_written) & 0xFF);
        result = ICM20948_WriteReg(mem_addr_reg, tmp[0]);
        if (result != 0) return result;
        result = ICM20948_WriteReg(mem_addr_reg + 1, tmp[1]);
        if (result != 0) return result;
        
        // 写入单个字节
        result = ICM20948_WriteReg(mem_data_reg, data[bytes_written]);
        if (result != 0) return result;
        
        bytes_written++;
    }
    
    return 0;
}

/**
 * @brief 从DMP程序内存读取数据
 * @param memory_addr 内存地址
 * @param data 数据缓冲区
 * @param size 数据大小(字节)
 * @return 0: 成功, 非0: 错误代码
 */
int8_t ICM20948_ReadMemory(uint16_t memory_addr, uint8_t *data, uint16_t size) {
    int8_t result;
    uint8_t bank;
    uint8_t mem_addr_reg;
    uint8_t mem_data_reg;
    uint8_t tmp[2];
    
    // 基于内存地址，选择正确的寄存器
    if (memory_addr < 0x2000) {
        // DMP程序内存
        bank = USER_BANK_0;
        mem_addr_reg = 0x7C; // MEM_START_ADDR
        mem_data_reg = 0x7D; // MEM_R_W
    } else {
        // 地址超出范围
        return -1;
    }
    
    // 设置内存银行
    ICM20948_SwitchBank(bank);
    
    // 分块读取
    uint16_t bytes_read = 0;
    while (bytes_read < size) {
        // 设置内存地址
        tmp[0] = (uint8_t)((memory_addr + bytes_read) >> 8);
        tmp[1] = (uint8_t)((memory_addr + bytes_read) & 0xFF);
        result = ICM20948_WriteReg(mem_addr_reg, tmp[0]);
        if (result != 0) return result;
        result = ICM20948_WriteReg(mem_addr_reg + 1, tmp[1]);
        if (result != 0) return result;
        
        // 读取单个字节
        result = ICM20948_ReadReg(mem_data_reg, &data[bytes_read]);
        if (result != 0) return result;
        
        bytes_read++;
    }
    
    return 0;
} 